
/***************************************************************************************
****************************************************************************************
* FILE		: vibration.c
* Description	: 
*			  
* Copyright (c) 2020 by Simlife. All Rights Reserved.
* 
* History:
* Version		Name			Date				Description
   0.1			Alen			2020/05/07			Initial Version
   
****************************************************************************************
****************************************************************************************/

#include "includes.h"

void initVibrationControlPin(void)
{
	motor1PortTypeControl &= motor1PinReverse;	 // output
	motor1PortPullupControl &= motor1PinReverse; // no pull up
	MOTOR1_OFF();

	motor2PortTypeControl &= motor2PinReverse;	 // output
	motor2PortPullupControl &= motor2PinReverse; // no pull up
	MOTOR2_OFF();

	motor3PortTypeControl &= motor3PinReverse;	 // output
	motor3PortPullupControl &= motor3PinReverse; // no pull up
	MOTOR3_OFF();

	motor4PortTypeControl &= motor4PinReverse;	 // output
	motor4PortPullupControl &= motor4PinReverse; // no pull up
	MOTOR4_OFF();

	motor5PortTypeControl &= motor5PinReverse;	 // output
	motor5PortPullupControl &= motor5PinReverse; // no pull up
	MOTOR5_OFF();

	motor6PortTypeControl &= motor6PinReverse;	 // output
	motor6PortPullupControl &= motor6PinReverse; // no pull up
	MOTOR6_OFF();

	motor7PortTypeControl &= motor7PinReverse;	 // output
	motor7PortPullupControl &= motor7PinReverse; // no pull up
	MOTOR7_OFF();

	motor8PortTypeControl &= motor8PinReverse;	 // output
	motor8PortPullupControl &= motor8PinReverse; // no pull up
	MOTOR8_OFF();

	motor9PortTypeControl &= motor9PinReverse;	 // output
	motor9PortPullupControl &= motor9PinReverse; // no pull up
	MOTOR9_OFF();

	motor10PortTypeControl &= motor10PinReverse;   // output
	motor10PortPullupControl &= motor10PinReverse; // no pull up
	MOTOR10_OFF();

	motor11PortTypeControl &= motor11PinReverse;   // output
	motor11PortPullupControl &= motor11PinReverse; // no pull up
	MOTOR11_OFF();

	motor12PortTypeControl &= motor12PinReverse;   // output
	motor12PortPullupControl &= motor12PinReverse; // no pull up
	MOTOR12_OFF();
}

void closeVibrationAllPin(void)
{
	MOTOR1_OFF();
	MOTOR2_OFF();
	MOTOR3_OFF();
	MOTOR4_OFF();
	MOTOR5_OFF();
	MOTOR6_OFF();
	MOTOR7_OFF();
	MOTOR8_OFF();
	MOTOR9_OFF();
	MOTOR10_OFF();
	MOTOR11_OFF();
	MOTOR12_OFF();
}

#define PWMPeriod 9

const unsigned char vibrationPWM[4] = {0, 2, 4, 9};

void controlMotorPwm(void)
{
	if (stVibration.countVibrationPWM < (stVibration.motor1PWM))
	{
		MOTOR1_ON();
	}
	else
	{
		MOTOR1_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor2PWM))
	{
		MOTOR2_ON();
	}
	else
	{
		MOTOR2_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor3PWM))
	{
		MOTOR3_ON();
	}
	else
	{
		MOTOR3_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor4PWM))
	{
		MOTOR4_ON();
	}
	else
	{
		MOTOR4_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor5PWM))
	{
		MOTOR5_ON();
	}
	else
	{
		MOTOR5_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor6PWM))
	{
		MOTOR6_ON();
	}
	else
	{
		MOTOR6_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor7PWM))
	{
		MOTOR7_ON();
	}
	else
	{
		MOTOR7_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor8PWM))
	{
		MOTOR8_ON();
	}
	else
	{
		MOTOR8_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor9PWM))
	{
		MOTOR9_ON();
	}
	else
	{
		MOTOR9_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor10PWM))
	{
		MOTOR10_ON();
	}
	else
	{
		MOTOR10_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor11PWM))
	{
		MOTOR11_ON();
	}
	else
	{
		MOTOR11_OFF();
	}

	if (stVibration.countVibrationPWM < (stVibration.motor12PWM))
	{
		MOTOR12_ON();
	}
	else
	{
		MOTOR12_OFF();
	}
}

void vibrationPWMProcess(void) //200us
{
	if (stSystem.vibrationMode == VIBRATION_MODE_OFF)
	{
		return;
	}

	if (stVibration.countVibrationPWM == vibrationPWM[0] || stVibration.countVibrationPWM == vibrationPWM[1] || stVibration.countVibrationPWM == vibrationPWM[2] || stVibration.countVibrationPWM == vibrationPWM[3])
	{
		controlMotorPwm();
	}

	stVibration.countVibrationPWM++;
	if (stVibration.countVibrationPWM > PWMPeriod)
	{
		stVibration.countVibrationPWM = 0;
	}
}

#define vibration1MaxLength 378 //345
const unsigned char vibrationMode1[][3] = {
	// 250ms call
	//		PWM
	//	1-4		  5 6	7 8 	9-12
	{B0000_0000, B0011_0000, B0000_0011},
	{B0000_0000, B0101_0000, B0000_0101},
	{B0000_0000, B1100_0000, B0000_1100},
	{B0000_0000, B0100_0000, B0000_0100},
	{B0000_0001, B0100_0000, B0001_0100},
	{B0000_0011, B0000_0000, B0011_0000},
	{B0000_0101, B0000_0000, B0101_0000},
	{B0000_1100, B0000_0000, B1100_0000},
	{B0001_0100, B0000_0001, B0100_0000},
	{B0011_0000, B0000_0011, B0000_0000},
	{B0101_0000, B0000_0101, B0000_0000},
	{B1100_0000, B0000_1100, B0000_0000},
	{B0100_0000, B0000_0100, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0000},

	{B0000_0000, B0001_0000, B0000_0001},
	{B0000_0000, B0111_0000, B0000_0111},
	{B0000_0001, B1101_0000, B0001_1101},
	{B0000_0111, B0100_0000, B0111_0100},
	{B0001_1101, B0000_0001, B1101_0000},
	{B0111_0100, B0000_0111, B0100_0000},
	{B1101_0000, B0001_1101, B0000_0001},
	{B0100_0000, B0111_0100, B0000_0111},
	{B0000_0001, B1101_0000, B0001_1101},
	{B0000_0111, B0100_0000, B0111_0100},
	{B0001_1101, B0000_0001, B1101_0000},
	{B0111_0100, B0000_0111, B0100_0000},
	{B1101_0000, B0000_1101, B0000_0000},
	{B0100_0000, B0000_0100, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0000}, //1

	{B0100_0000, B0000_0100, B0000_0000},
	{B1100_0000, B0000_1100, B0000_0000},
	{B0101_0000, B0000_0101, B0000_0000},
	{B0011_0000, B0000_0011, B0000_0000},
	{B0001_0100, B0000_0001, B0100_0000},
	{B0000_1100, B0000_0000, B1100_0000},
	{B0000_0101, B0000_0000, B0101_0000},
	{B0000_0011, B0000_0000, B0011_0000},
	{B0000_0001, B0100_0000, B0001_0100},
	{B0000_0000, B1100_0000, B0000_1100},
	{B0000_0000, B0101_0000, B0000_0101},
	{B0000_0000, B0011_0000, B0000_0011},
	{B0000_0000, B0001_0000, B0000_0001},
	{B0000_0000, B0000_0000, B0000_0000},

	{B0100_0000, B0000_0100, B0000_0000},
	{B1101_0000, B0000_1101, B0000_0000},
	{B0111_0100, B0000_0111, B0100_0000},
	{B0001_1101, B0000_0001, B1101_0000},
	{B0000_0111, B0100_0000, B0111_0100},
	{B0000_0001, B1101_0000, B0001_1101},
	{B0100_0000, B0111_0100, B0000_0111},
	{B1101_0000, B0001_1101, B0000_0001},
	{B0111_0100, B0000_0111, B0100_0000},
	{B0001_1101, B0000_0001, B1101_0000},
	{B0000_0111, B0100_0000, B0111_0100},
	{B0000_0001, B1101_0000, B0001_1101},
	{B0000_0000, B0111_0000, B0000_0111},
	{B0000_0000, B0001_0000, B0000_0001},
	{B0000_0000, B0000_0000, B0000_0000}, //2

	{B0000_0000, B0000_0000, B0000_0001},
	{B0000_0000, B0000_0000, B0000_0111},
	{B0000_0000, B0000_0000, B0001_1101},
	{B0000_0000, B0000_0000, B0111_0100},
	{B0000_0000, B0000_0001, B1101_0000},
	{B0000_0000, B0000_0111, B0100_0000},
	{B0100_0000, B0000_1101, B0000_0000},
	{B1101_0000, B0000_0100, B0000_0000},
	{B0111_0100, B0000_0000, B0000_0000},
	{B0001_1101, B0000_0000, B0000_0000},
	{B0000_0111, B0100_0000, B0000_0000},
	{B0000_0001, B1101_0000, B0000_0000},
	{B0000_0000, B0111_0000, B0000_0001},
	{B0000_0000, B0001_0000, B0000_0111},
	{B0000_0000, B0000_0000, B0001_1101},
	{B0000_0000, B0000_0000, B0111_0100},
	{B0000_0000, B0000_0001, B1101_0000},
	{B0000_0000, B0000_0111, B0100_0000},
	{B0100_0000, B0000_1101, B0000_0000},
	{B1101_0000, B0000_0100, B0000_0000},
	{B0111_0100, B0000_0000, B0000_0000},
	{B0001_1101, B0000_0000, B0000_0000},
	{B0000_0111, B0100_0000, B0000_0000},
	{B0000_0001, B1101_0000, B0000_0000},
	{B0000_0000, B0111_0000, B0000_0000},
	{B0000_0000, B0001_0000, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0000}, //3

	{B0100_0000, B0000_0000, B0000_0000},
	{B1101_0000, B0000_0000, B0000_0000},
	{B0111_0100, B0000_0000, B0000_0000},
	{B0001_1101, B0000_0000, B0000_0000},
	{B0000_0111, B0100_0000, B0000_0000},
	{B0000_0001, B1101_0000, B0000_0000},
	{B0000_0000, B0111_0000, B0000_0001},
	{B0000_0000, B0001_0000, B0000_0111},
	{B0000_0000, B0000_0000, B0001_1101},
	{B0000_0000, B0000_0000, B0111_0100},
	{B0000_0000, B0000_0001, B1101_0000},
	{B0000_0000, B0000_0111, B0100_0000},
	{B0100_0000, B0000_1101, B0000_0000},
	{B1101_0000, B0000_0100, B0000_0000},
	{B0111_0100, B0000_0000, B0000_0000},
	{B0001_1101, B0000_0000, B0000_0000},
	{B0000_0111, B0100_0000, B0000_0000},
	{B0000_0001, B1101_0000, B0000_0000},
	{B0000_0000, B0111_0000, B0000_0001},
	{B0000_0000, B0001_0000, B0000_0111},
	{B0000_0000, B0000_0000, B0001_1101},
	{B0000_0000, B0000_0000, B0111_0100},
	{B0000_0000, B0000_0001, B1101_0000},
	{B0000_0000, B0000_0111, B0100_0000},
	{B0000_0000, B0000_1101, B0000_0000},
	{B0000_0000, B0000_0100, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0000}, //4

	{B0000_0000, B0000_0000, B0101_0000},
	{B0000_0000, B0000_0000, B1111_0000},
	{B0000_0000, B0000_0001, B0101_0100},
	{B0000_0000, B0000_0011, B0000_1100},
	{B0000_0000, B0000_0101, B0000_0101},
	{B0000_0000, B0000_1100, B0000_0011},
	{B0100_0000, B0001_0100, B0000_0001},
	{B1100_0000, B0011_0000, B0000_0000},
	{B0101_0000, B0101_0000, B0000_0000},
	{B0011_0000, B1100_0000, B0000_0000},
	{B0001_0101, B0100_0000, B0000_0000},
	{B0000_1111, B0000_0000, B0000_0000},
	{B0000_0101, B0000_0000, B0101_0000},
	{B0000_0000, B0000_0000, B1111_0000},
	{B0000_0000, B0000_0001, B0101_0100},
	{B0000_0000, B0000_0011, B0000_1100},
	{B0000_0000, B0000_0101, B0000_0101},
	{B0000_0000, B0000_1100, B0000_0011},
	{B0100_0000, B0001_0100, B0000_0001},
	{B1100_0000, B0011_0000, B0000_0000},
	{B0101_0000, B0101_0000, B0000_0000},
	{B0011_0000, B1100_0000, B0000_0000},
	{B0001_0101, B0100_0000, B0000_0000},
	{B0000_1111, B0000_0000, B0000_0000},
	{B0001_0101, B0100_0000, B0000_0000},
	{B0011_0000, B1100_0000, B0000_0000},
	{B0101_0000, B0101_0000, B0000_0000},
	{B1100_0000, B0011_0000, B0000_0000},
	{B0100_0000, B0001_0100, B0000_0001},
	{B0000_0000, B0000_1100, B0000_0011},
	{B0000_0000, B0000_0101, B0000_0101},
	{B0000_0000, B0000_0011, B0000_1100},
	{B0000_0000, B0000_0001, B0101_0100},
	{B0000_0000, B0000_0000, B1111_0000},
	{B0000_0101, B0000_0000, B0101_0000},
	{B0000_1111, B0000_0000, B0000_0000},
	{B0001_0101, B0100_0000, B0000_0000},
	{B0011_0000, B1100_0000, B0000_0000},
	{B0101_0000, B0101_0000, B0000_0000},
	{B1100_0000, B0011_0000, B0000_0000},
	{B0100_0000, B0001_0100, B0000_0001},
	{B0000_0000, B0000_1100, B0000_0011},
	{B0000_0000, B0000_0101, B0000_0101},
	{B0000_0000, B0000_0011, B0000_1100},
	{B0000_0000, B0000_0001, B0101_0100},
	{B0000_0000, B0000_0000, B1111_0000},
	{B0000_0000, B0000_0000, B0101_0000},
	{B0000_0000, B0000_0000, B0000_0000},

	{B0000_0000, B0000_0000, B0101_0000},
	{B0000_0000, B0000_0001, B1111_0100},
	{B0000_0000, B0000_0111, B0101_1101},
	{B0100_0000, B0001_1101, B0000_0111},
	{B1101_0000, B0111_0100, B0000_0001},
	{B0111_0101, B1101_0000, B0000_0000},
	{B0001_1111, B0100_0000, B0101_0000},
	{B0000_0101, B0000_0001, B1111_0100},
	{B0000_0000, B0000_0111, B0101_1101},
	{B0100_0000, B0001_1101, B0000_0111},
	{B1101_0000, B0111_0100, B0000_0000},
	{B0111_0101, B1101_0000, B0000_0000},
	{B0001_1111, B0100_0000, B0000_0000},
	{B0111_0101, B1101_0000, B0000_0000},
	{B1101_0000, B0111_0100, B0000_0001},
	{B0100_0000, B0001_1101, B0000_0111},
	{B0000_0000, B0000_0111, B0101_1101},
	{B0000_0101, B0000_0001, B1111_0100},
	{B0001_1111, B0100_0000, B0101_0000},
	{B0111_0101, B1101_0000, B0000_0000},
	{B1101_0000, B0111_0100, B0000_0001},
	{B0100_0000, B0001_1101, B0000_0111},
	{B0000_0000, B0000_0111, B0101_1101},
	{B0000_0000, B0000_0001, B1111_0100},
	{B0000_0000, B0000_0000, B0101_0000},
	{B0000_0000, B0000_0000, B0000_0000}, //5

	{B0000_0000, B0000_0000, B0100_0000},
	{B0000_0000, B0000_0000, B1100_0000},
	{B0000_0000, B0000_0001, B0100_0000},
	{B0000_0000, B0000_0011, B0000_0000},
	{B0000_0000, B0000_0101, B0000_0000},
	{B0000_0000, B0000_1100, B0000_0000},
	{B0100_0000, B0000_0100, B0000_0000},
	{B1100_0000, B0000_0000, B0000_0000},
	{B0101_0000, B0000_0000, B0000_0000},
	{B0011_0000, B0000_0000, B0000_0000},
	{B0001_0100, B0000_0000, B0000_0000},
	{B0000_1100, B0000_0000, B0000_0000},
	{B0000_0101, B0000_0000, B0000_0000},
	{B0000_0011, B0000_0000, B0000_0000},
	{B0000_0001, B0100_0000, B0000_0000},
	{B0000_0000, B1100_0000, B0000_0000},
	{B0000_0000, B0101_0000, B0000_0000},
	{B0000_0000, B0011_0000, B0000_0000},
	{B0000_0000, B0001_0000, B0000_0001},
	{B0000_0000, B0000_0000, B0000_0011},
	{B0000_0000, B0000_0000, B0000_0101},
	{B0000_0000, B0000_0000, B0000_1100},
	{B0000_0000, B0000_0000, B0001_0100},
	{B0000_0000, B0000_0000, B0011_0000},
	{B0000_0000, B0000_0000, B0001_0000},
	{B0000_0000, B0000_0000, B0000_0000},

	{B0000_0000, B0000_0000, B0100_0000},
	{B0000_0000, B0000_0001, B1100_0000},
	{B0000_0000, B0000_0111, B0100_0000},
	{B0100_0000, B0000_1101, B0000_0000},
	{B1101_0000, B0000_0100, B0000_0000},
	{B0111_0100, B0000_0000, B0000_0000},
	{B0001_1101, B0000_0000, B0000_0000},
	{B0000_0111, B0100_0000, B0000_0000},
	{B0000_0001, B1101_0000, B0000_0000},
	{B0000_0000, B0111_0000, B0000_0001},
	{B0000_0000, B0001_0000, B0000_0111},
	{B0000_0000, B0000_0000, B0001_1101},
	{B0000_0000, B0000_0000, B0011_0100},
	{B0000_0000, B0000_0000, B0001_0000},
	{B0000_0000, B0000_0000, B0000_0000},
	{B0000_0000, B0000_0000, B0100_0000},
	{B0000_0000, B0000_0001, B1100_0000},
	{B0000_0000, B0000_0111, B0100_0000},
	{B0100_0000, B0000_1101, B0000_0000},
	{B1101_0000, B0000_0100, B0000_0000},
	{B0111_0100, B0000_0000, B0000_0000},
	{B0001_1101, B0000_0000, B0000_0000},
	{B0000_0111, B0100_0000, B0000_0000},
	{B0000_0001, B1101_0000, B0000_0000},
	{B0000_0000, B0111_0000, B0000_0001},
	{B0000_0000, B0001_0000, B0000_0111},
	{B0000_0000, B0000_0000, B0001_1101},
	{B0000_0000, B0000_0000, B0011_0100},
	{B0000_0000, B0000_0000, B0001_0000},
	{B0000_0000, B0000_0000, B0000_0000}, //6

	{B0000_0100, B0000_0000, B0000_0000},
	{B0000_1100, B0000_0000, B0000_0000},
	{B0001_0100, B0000_0000, B0000_0000},
	{B0011_0000, B0000_0000, B0000_0000},
	{B0101_0000, B0000_0000, B0000_0000},
	{B1100_0000, B0000_0000, B0000_0000},
	{B0100_0000, B0000_0100, B0000_0000},
	{B0000_0000, B0000_1100, B0000_0000},
	{B0000_0000, B0000_0101, B0000_0000},
	{B0000_0000, B0000_0011, B0000_0000},
	{B0000_0000, B0000_0001, B0100_0000},
	{B0000_0000, B0000_0000, B1100_0000},
	{B0000_0000, B0000_0000, B0101_0000},
	{B0000_0000, B0000_0000, B0011_0000},
	{B0000_0000, B0000_0000, B0001_0100},
	{B0000_0000, B0000_0000, B0000_1100},
	{B0000_0000, B0000_0000, B0000_0101},
	{B0000_0000, B0000_0000, B0000_0011},
	{B0000_0000, B0001_0000, B0000_0001},
	{B0000_0000, B0011_0000, B0000_0000},
	{B0000_0000, B0101_0000, B0000_0000},
	{B0000_0000, B1100_0000, B0000_0000},
	{B0000_0001, B0100_0000, B0000_0000},
	{B0000_0011, B0000_0000, B0000_0000},
	{B0000_0001, B0000_0000, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0000},

	{B0000_0100, B0000_0000, B0000_0000},
	{B0001_1100, B0000_0000, B0000_0000},
	{B0111_0100, B0000_0000, B0000_0000},
	{B1101_0000, B0000_0100, B0000_0000},
	{B0100_0000, B0000_1101, B0000_0000},
	{B0000_0000, B0000_0111, B0100_0000},
	{B0000_0000, B0000_0001, B1101_0000},
	{B0000_0000, B0000_0000, B0111_0100},
	{B0000_0000, B0000_0000, B0001_1101},
	{B0000_0000, B0001_0000, B0000_0111},
	{B0000_0000, B0111_0000, B0000_0001},
	{B0000_0001, B1101_0000, B0000_0000},
	{B0000_0011, B0100_0000, B0000_0000},
	{B0000_0001, B0000_0000, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0000},
	{B0000_0100, B0000_0000, B0000_0000},
	{B0001_1100, B0000_0000, B0000_0000},
	{B0111_0100, B0000_0000, B0000_0000},
	{B1101_0000, B0000_0100, B0000_0000},
	{B0100_0000, B0000_1101, B0000_0000},
	{B0000_0000, B0000_0111, B0100_0000},
	{B0000_0000, B0000_0001, B1101_0000},
	{B0000_0000, B0000_0000, B0111_0100},
	{B0000_0000, B0000_0000, B0001_1101},
	{B0000_0000, B0001_0000, B0000_0111},
	{B0000_0000, B0111_0000, B0000_0001},
	{B0000_0001, B1101_0000, B0000_0000},
	{B0000_0011, B0100_0000, B0000_0000},
	{B0000_0001, B0000_0000, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0000}, //7

	{B0000_0000, B0000_0000, B1100_0000},
	{B0000_0000, B0000_0000, B0111_0000},
	{B0000_0000, B0000_0000, B0001_1100},
	{B0000_0000, B0000_0011, B0000_0100},
	{B0000_0000, B0000_1101, B0000_0000},
	{B0000_0000, B0000_0100, B0000_0011},
	{B0000_0000, B0011_0000, B0000_0001},
	{B1100_0000, B0001_0000, B0000_0000},
	{B0111_0000, B0000_0000, B0000_0000},
	{B0001_0000, B1100_0000, B0000_0000},
	{B0000_0011, B0100_0000, B0000_0000},
	{B0000_1101, B0000_0000, B0000_0000},
	{B0000_0111, B0000_0000, B0000_0000},
	{B0000_0001, B1100_0000, B0000_0000},
	{B0011_0000, B0100_0000, B0000_0000},
	{B1101_0000, B0000_0000, B0000_0000},
	{B0100_0000, B0011_0000, B0000_0000},
	{B0000_0000, B0001_0000, B0000_0011},
	{B0000_0000, B0000_1100, B0000_0001},
	{B0000_0000, B0000_0111, B0000_0000},
	{B0000_0000, B0000_0001, B0000_1100},
	{B0000_0000, B0000_0000, B0011_0100},
	{B0000_0000, B0000_0000, B1101_0000},
	{B0000_0000, B0000_0000, B0100_0000},
	{B0000_0000, B0000_0000, B0000_1100},
	{B0000_0000, B0000_1100, B0000_0000},
	{B0000_0000, B0011_0000, B0000_0000},
	{B0011_0000, B0000_0000, B0000_0000},
	{B0000_0011, B0000_0000, B0000_0000},
	{B0000_1100, B0000_0000, B0000_0000},
	{B0000_0000, B1100_0000, B0000_0000},
	{B1100_0000, B0000_0000, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0011},
	{B0000_0000, B0000_0011, B0000_0000},
	{B0000_0000, B0000_0000, B0011_0000},
	{B0000_0000, B0000_0000, B0000_1100},
	{B0000_0000, B0000_1100, B0000_0000},
	{B0000_0000, B0011_0000, B0000_0000},
	{B0011_0000, B0000_0000, B0000_0000},
	{B0000_0011, B0000_0000, B0000_0000},
	{B0000_1100, B0000_0000, B0000_0000},
	{B0000_0000, B1100_0000, B0000_0000},
	{B1100_0000, B0000_0000, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0011},
	{B0000_0000, B0000_0011, B0000_0000},
	{B0000_0000, B0000_0000, B0011_0000},
	{B0000_0000, B0000_0000, B0000_0000}, //8

	{B0000_0010, B0000_0000, B1000_0000},
	{B0000_0011, B1000_0010, B1100_0000},
	{B0000_0001, B1110_1011, B0100_0000},
	{B1000_0000, B0111_1101, B0000_0010},
	{B1110_0000, B0001_0100, B0000_1011},
	{B0111_1000, B0000_0000, B0010_1101},
	{B0001_1100, B0000_0000, B0011_0100},
	{B0000_0100, B0000_0000, B0001_0000},

	{B0000_0010, B0000_0000, B1000_0000},
	{B0000_0011, B1000_0010, B1100_0000},
	{B0000_0001, B1110_1011, B0100_0000},
	{B1000_0000, B0111_1101, B0000_0010},
	{B1110_0000, B0001_0100, B0000_1011},
	{B0111_1000, B0000_0000, B0010_1101},
	{B0001_1100, B0000_0000, B0011_0100},
	{B0000_0100, B0000_0000, B0001_0000},

	{B0000_1000, B0000_0000, B0010_0000},
	{B0010_1100, B0000_0000, B0011_1000},
	{B1011_0100, B0000_0000, B0001_1110},
	{B1101_0000, B0010_1000, B0000_0111},
	{B0100_0000, B1011_1110, B0000_0001},
	{B0000_0010, B1101_0111, B1000_0000},
	{B0000_0011, B0100_0001, B1100_0000},
	{B0000_0001, B0000_0000, B0100_0000},

	{B0000_1000, B0000_0000, B0010_0000},
	{B0010_1100, B0000_0000, B0011_1000},
	{B1011_0100, B0000_0000, B0001_1110},
	{B1101_0000, B0010_1000, B0000_0111},
	{B0100_0000, B1011_1110, B0000_0001},
	{B0000_0010, B1101_0111, B1000_0000},
	{B0000_0011, B0100_0001, B1100_0000},
	{B0000_0001, B0000_0000, B0100_0000},

	{B0000_0000, B0000_0000, B0000_0000}, //9

};

#define vibration2MaxLength 85
const unsigned char vibrationMode2[][3] = {
	//	PWM
	//	 1-4	  5 6   7 8      9-12
	{B0000_0000, B0000_0000, B1111_0000},
	{B0000_0000, B0000_0011, B0000_1100},
	{B0000_0000, B0000_1100, B0000_0011},
	{B1100_0000, B0011_0000, B0000_0000},
	{B0011_0000, B1100_0000, B0000_0000},
	{B0000_1111, B0000_0000, B0000_0000},
	{B0011_0000, B1100_0000, B0000_0000},
	{B1100_0000, B0111_0000, B0000_0000},
	{B0000_0000, B0000_1100, B0000_0011},
	{B0000_0000, B0000_0011, B0000_1100},
	{B0000_0000, B0000_0000, B1111_0000},
	{B0000_0000, B0000_0000, B0000_0000},

	{B0000_1111, B0000_0000, B1111_0000},
	{B0011_0000, B1100_0011, B0000_1100},
	{B1100_0000, B0011_1100, B0000_0011},
	{B0011_0000, B1100_0011, B0000_1100},
	{B0000_1111, B0000_0000, B1111_0000},
	{B0011_0000, B1100_0011, B0000_1100},
	{B1100_0000, B0011_1100, B0000_0011},
	{B0011_0000, B1100_0011, B0000_1100},
	{B0000_1111, B0000_0000, B1111_0000},
	{B0000_0000, B0000_0000, B0000_0000},

	{B0000_0000, B0011_0000, B0000_0011},
	{B0000_0000, B1100_0000, B0000_1100},
	{B0000_0011, B0000_0000, B0011_0000},
	{B0000_1100, B0000_0000, B1100_0000},
	{B0011_0000, B0000_0011, B0000_0000},
	{B1100_0000, B0000_1100, B0000_0000},
	{B0011_0000, B0000_0011, B0000_0000},
	{B0000_1100, B0000_0000, B1100_0000},
	{B0000_0011, B0000_0000, B0011_0000},
	{B0000_0000, B1100_0000, B0000_1100},
	{B0000_0000, B0011_0000, B0000_0011},
	{B0000_0000, B1100_0000, B0000_1100},
	{B0000_0011, B0000_0000, B0011_0000},
	{B0000_1100, B0000_0000, B1100_0000},
	{B0011_0000, B0000_0011, B0000_0000},
	{B1100_0000, B0000_1100, B0000_0000},
	{B0011_0000, B0000_0011, B0000_0000},
	{B0000_1100, B0000_0000, B1100_0000},
	{B0000_0011, B0000_0000, B0011_0000},
	{B0000_0000, B1100_0000, B0000_1100},
	{B0000_0000, B0011_0000, B0000_0011},
	{B0000_0000, B0000_0000, B0000_0000},

	{B0000_0000, B0000_0000, B1100_0000},
	{B0000_0000, B0000_0000, B0000_1100},
	{B0000_0000, B0000_1100, B0000_0000},
	{B0000_0000, B0011_0000, B0000_0000},
	{B0011_0000, B0000_0000, B0000_0000},
	{B0000_0011, B0000_0000, B0000_0000},
	{B0000_1100, B0000_0000, B0000_0000},
	{B0000_0000, B1100_0000, B0000_0000},
	{B1100_0000, B0000_0000, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0011},
	{B0000_0000, B0000_0011, B0000_0000},
	{B0000_0000, B0000_0000, B0011_0000},
	{B0000_0000, B0000_0000, B1100_0000},
	{B0000_0000, B0000_0000, B0000_1100},
	{B0000_0000, B0000_1100, B0000_0000},
	{B0000_0000, B0011_0000, B0000_0000},
	{B0011_0000, B0000_0000, B0000_0000},
	{B0000_0011, B0000_0000, B0000_0000},
	{B0000_1100, B0000_0000, B0000_0000},
	{B0000_0000, B1100_0000, B0000_0000},
	{B1100_0000, B0000_0000, B0000_0000},
	{B0000_0000, B0000_0000, B0000_0011},
	{B0000_0000, B0000_0011, B0000_0000},
	{B0000_0000, B0000_0000, B0011_0000},
	{B0000_0000, B0000_0000, B0000_0000},

	{B0000_0010, B0000_0000, B1000_0000},
	{B0000_0011, B1000_0010, B1100_0000},
	{B0000_0001, B1110_1011, B0100_0000},
	{B1000_0000, B0111_1101, B0000_0010},
	{B1110_0000, B0001_0100, B0000_1011},
	{B0111_1000, B0000_0000, B0010_1101},
	{B0001_1100, B0000_0000, B0011_0100},
	{B0000_0100, B0000_0000, B0001_0000},

	{B0000_1000, B0000_0000, B0010_0000},
	{B0010_1100, B0000_0000, B0011_1000},
	{B1011_0100, B0000_0000, B0001_1110},
	{B1101_0000, B0010_1000, B0000_0111},
	{B0100_0000, B1011_1110, B0000_0001},
	{B0000_0010, B1101_0111, B1000_0000},
	{B0000_0011, B0100_0001, B1100_0000},
	{B0000_0001, B0000_0000, B0100_0000},

};

#define vibration3MaxLength 12
const unsigned char vibrationMode3[][3] = {
	//	PWM
	//	 1-4	  5 6   7 8      9-12
	{B1100_0000, B0000_0000, B0000_0000},
	{B0011_0000, B0000_0000, B0000_0000},
	{B0000_1100, B0000_0000, B0000_0000},
	{B0000_0011, B0000_0000, B0000_0000},
	{B0000_0000, B1100_0000, B0000_0000},
	{B0000_0000, B0011_0000, B0000_0000},
	{B0000_0000, B0000_1100, B0000_0000},
	{B0000_0000, B0000_0011, B0000_0000},
	{B0000_0000, B0000_0000, B1100_0000},
	{B0000_0000, B0000_0000, B0011_0000},
	{B0000_0000, B0000_0000, B0000_1100},
	{B0000_0000, B0000_0000, B0000_0011},
};

void vibrationProcess(void) //250ms call
{
	unionUnsignedCharTwo bitsTwo;
	if (stSystem.vibrationMode == VIBRATION_MODE_OFF)
	{
		stVibration.countVibration = 0;
		stVibration.countVibrationPWM = 0;
	}
	else
	{
		stVibration.countVibrationPWM = 0;
		if (stSystem.vibrationMode == VIBRATION_MODE1)
		{
			bitsTwo.byte = vibrationMode1[stVibration.countVibration][0];
		}
		else if (stSystem.vibrationMode == VIBRATION_MODE2)
		{
			bitsTwo.byte = vibrationMode2[stVibration.countVibration][0];
		}
		else if (stSystem.vibrationMode == VIBRATION_MODE3)
		{
			bitsTwo.byte = vibrationMode3[stVibration.countVibration][0];
		}

		stVibration.motor1PWM = vibrationPWM[bitsTwo.bits.b4];
		stVibration.motor2PWM = vibrationPWM[bitsTwo.bits.b3];
		stVibration.motor3PWM = vibrationPWM[bitsTwo.bits.b2];
		stVibration.motor4PWM = vibrationPWM[bitsTwo.bits.b1];

		if (stSystem.vibrationMode == VIBRATION_MODE1)
		{
			bitsTwo.byte = vibrationMode1[stVibration.countVibration][1];
		}
		else if (stSystem.vibrationMode == VIBRATION_MODE2)
		{
			bitsTwo.byte = vibrationMode2[stVibration.countVibration][1];
		}
		else if (stSystem.vibrationMode == VIBRATION_MODE3)
		{
			bitsTwo.byte = vibrationMode3[stVibration.countVibration][1];
		}

		stVibration.motor5PWM = vibrationPWM[bitsTwo.bits.b4];
		stVibration.motor6PWM = vibrationPWM[bitsTwo.bits.b3];
		stVibration.motor7PWM = vibrationPWM[bitsTwo.bits.b2];
		stVibration.motor8PWM = vibrationPWM[bitsTwo.bits.b1];

		if (stSystem.vibrationMode == VIBRATION_MODE1)
		{
			bitsTwo.byte = vibrationMode1[stVibration.countVibration][2];
		}
		else if (stSystem.vibrationMode == VIBRATION_MODE2)
		{
			bitsTwo.byte = vibrationMode2[stVibration.countVibration][2];
		}
		else if (stSystem.vibrationMode == VIBRATION_MODE3)
		{
			bitsTwo.byte = vibrationMode3[stVibration.countVibration][2];
		}
		stVibration.motor9PWM = vibrationPWM[bitsTwo.bits.b4];
		stVibration.motor10PWM = vibrationPWM[bitsTwo.bits.b3];
		stVibration.motor11PWM = vibrationPWM[bitsTwo.bits.b2];
		stVibration.motor12PWM = vibrationPWM[bitsTwo.bits.b1];

		stVibration.countVibration++;
		if (stSystem.vibrationMode == VIBRATION_MODE1)
		{
			if (stVibration.countVibration == vibration1MaxLength)
			{
				stVibration.countVibration = 0;
			}
		}
		else if (stSystem.vibrationMode == VIBRATION_MODE2)
		{
			if (stVibration.countVibration == vibration2MaxLength)
			{
				stVibration.countVibration = 0;
			}
		}
		else if (stSystem.vibrationMode == VIBRATION_MODE3)
		{
			if (stVibration.countVibration == vibration3MaxLength)
			{
				stVibration.countVibration = 0;
			}
		}
	}
}
